/* Includes ----------------------------------------------------------------- */
#include "../device/compass.h"

#include <i2c.h>

#include "../bsp/bsp_tim.h"
#include "main.h"
/* Private define ----------------------------------------------------------- */
#define DEVIATION_COMPASS_X (0.f)
#define DEVIATION_COMPASS_Y (0.f)
#define DEVIATION_COMPASS_Z (0.f)
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */

/* Private function  -------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */
void Compass_Init(Compass_t *raw) {
  const uint8_t t1 = 0x01, t2 = 0x40, t3 = 0x01, t4 = 0x0d;
  HAL_I2C_Mem_Write(&hi2c1, 0x1a, 0x0B, I2C_MEMADD_SIZE_8BIT, (uint8_t *)&t1,
                    sizeof(uint8_t), 100);
  HAL_I2C_Mem_Write(&hi2c1, 0x1a, 0x20, I2C_MEMADD_SIZE_8BIT, (uint8_t *)&t2,
                    sizeof(uint8_t), 100);
  HAL_I2C_Mem_Write(&hi2c1, 0x1a, 0x21, I2C_MEMADD_SIZE_8BIT, (uint8_t *)&t3,
                    sizeof(uint8_t), 100);
  HAL_I2C_Mem_Write(&hi2c1, 0x1a, 0x09, I2C_MEMADD_SIZE_8BIT, (uint8_t *)&t4,
                    sizeof(uint8_t), 100);
}

void Compass_GetData(Compass_t *raw) {
  uint8_t compass_tmp[6];
  HAL_I2C_Mem_Read(&hi2c1, 0x1a, 0x00, I2C_MEMADD_SIZE_8BIT,
                   (uint8_t *)compass_tmp, 6 * sizeof(uint8_t), 5);
  /* read raw compass data */
  if ((compass_tmp[1] << 8) + compass_tmp[0] > 0x7fff)
    raw->x = (compass_tmp[1] << 8) + compass_tmp[0] - 0xffff;
  else
    raw->x = (compass_tmp[1] << 8) + compass_tmp[0];
  if ((compass_tmp[3] << 8) + compass_tmp[2] > 0x7fff)
    raw->y = (compass_tmp[3] << 8) + compass_tmp[2] - 0xffff;
  else
    raw->y = (compass_tmp[3] << 8) + compass_tmp[2];
  if ((compass_tmp[5] << 8) + compass_tmp[4] > 0x7fff)
    raw->z = (compass_tmp[5] << 8) + compass_tmp[4] - 0xffff;
  else
    raw->z = (compass_tmp[5] << 8) + compass_tmp[4];
}
